//
// Created by bourne on 2024/4/15.
//

#ifndef SRC_JOY_H
#define SRC_JOY_H

#include <iostream>
#include <ros/ros.h>
#include <quadrotor_msgs/PositionCommand.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Joy.h>
#include <visualization_msgs/Marker.h>
#include <Eigen/Eigen>
#include <tf/tf.h>

class DroneJoyStick {
public:
  DroneJoyStick();
  ~DroneJoyStick();

  void init(ros::NodeHandle& nh);
private:
  // params
  int drone_id_; // 无人机编号
  int activate_button_; // 激活用的通道
  std::vector<int> activate_buttons_;
  std::vector<std::string> button_names_;
  double init_x_, init_y_, init_z_;

  // data
  bool activated_;
  Eigen::Vector3d pos_;
  double yaw_;

  Eigen::Vector4d joy_cmd_; // [for-back ward, left-right, up-down, yaw]
  quadrotor_msgs::PositionCommand cmd_;

  ros::NodeHandle nh_;
  ros::Subscriber odom_sub_, joy_sub_;
  ros::Publisher cmd_pub_, state_pub_;
  ros::Timer state_timer_, cmd_timer_;

  void odomCallback(const nav_msgs::OdometryConstPtr& msg);
  void joyCallback(const sensor_msgs::JoyConstPtr& msg);
  void stateTimerCallback(const ros::TimerEvent& e);
  void cmdTimerCallback(const ros::TimerEvent& e);
};

#endif //SRC_JOY_H
